/*
 * @author: wizardc
 */

using System.Collections.Generic;
using Dou.DataStructure;

namespace Dou.Pathfinding
{
    /// <summary>
    /// A 星寻路
    /// 注意：找到的路径为连续的格子
    /// </summary>
    public class AStar
    {
        private BinaryHeaps<AStarNode> _binaryHeaps = new BinaryHeaps<AStarNode>((node1, node2) => node2.f - node1.f);
        private List<AStarNode> _nodeList = new List<AStarNode>();

        /// <summary>
        /// 寻路
        /// </summary>
        /// <param name="graph">地图</param>
        /// <param name="path">找到的路径会加入该列表中</param>
        /// <returns>是否找到路径</returns>
        public bool Pathfinding(IAStarGraph graph, List<AStarNode> path)
        {
            var startNode = graph.GetStartNode();
            var endNode = graph.GetEndNode();
            
            graph.UpdateVersion();
            var version = graph.GetVersion();
            
            startNode.g = 0;
            startNode.h = graph.GetCost(startNode, endNode);
            startNode.f = startNode.g + startNode.h;

            // 寻路
            {
                var node = startNode;
                while (node != endNode)
                {
                    var aroundLinks = node.aroundLinks;
                    for (var i = 0; i < aroundLinks.Length; i++)
                    {
                        var test = aroundLinks[i].node;
                        var cost = aroundLinks[i].cost;
                        var g = node.g + cost;
                        var h = graph.GetCost(test, endNode);
                        var f = g + h;
                        if (test.version == version)
                        {
                            if (test.f > f)
                            {
                                test.f = f;
                                test.g = g;
                                test.h = h;
                                test.parent = node;
                                _binaryHeaps.Modify(test, test);
                            }
                        }
                        else
                        {
                            test.f = f;
                            test.g = g;
                            test.h = h;
                            test.parent = node;
                            _binaryHeaps.Enqueue(test);
                            test.version = version;
                        }
                    }
                    node.version = version;
                    if (_binaryHeaps.count == 0)
                    {
                        _binaryHeaps.Clear();
                        return false;
                    }
                    node = _binaryHeaps.Dequeue();
                }
                _binaryHeaps.Clear();
            }

            // 构建路径
            {
                var node = endNode;
                _nodeList.Add(node);
                while (node != startNode)
                {
                    node = node.parent;
                    _nodeList.Add(node);
                }
                _nodeList.Reverse();
                for (int i = 0; i < _nodeList.Count; i++)
                {
                    path.Add(_nodeList[i]);
                }
                _nodeList.Clear();
            }
            
            return true;
        }
    }

    public abstract class AStarNode
    {
        public bool walkable;

        internal float f;
        internal float g;
        internal float h;
        internal AStarNode parent;
        internal int version;
        internal AStarLink[] aroundLinks;
    }

    public class AStarLink
    {
        public AStarNode node;
        public float cost;
    }
}
